/*
 * Copyright 2003 Program of Computer Graphics, Cornell University
 *     580 Rhodes Hall
 *     Cornell University
 *     Ithaca NY 14853
 * Web: http://www.graphics.cornell.edu/
 * 
 * Not for commercial use. Do not redistribute without permission.
 */
#include "stdafx.h"

#include "Triangle.h"

int usedTriSlots = 0;
int unusedTriSlots = 0;

void Triangle::printSlotStats() {
  printf("compiled tri slots  used %d  free %d\n",usedTriSlots,unusedTriSlots);
}

PrimitiveList *Triangle::compile(int num,const GeometryObject **array,PoolAllocator *pool) const
{
//_asm int 3; //force breakpoint
  QuadPackedTriangleList *list = QuadPackedTriangleList::poolAlloc(num,pool);
  for(int k=0;k<num;k++) {
    assert(array[k]->getType()==GEOM_TRIANGLE);
    Triangle *t = (Triangle*)array[k];
    list->setTriangle(k,t,pool);
//    list->setTriangle(k,t->v0x,t->v0y,t->v0z,t->v1x,t->v1y,t->v1z,
//      t->v2x,t->v2y,t->v2z,t->id());
  }
#ifdef USE_COMPILED_SHARED_TRIANGLE
  //TODO: perhaps we should instead fill with copies of dummy triangle???
  //fill empty slots with extra copies of the first triangle
  int numEmpty = (4 - num)&0x03;
  for(int j=0;j<numEmpty;j++) {
    list->setTriangle(j+num,(Triangle*)array[0],pool);
  }
  //since quad packed triangles are store without the usual PrimitiveList header
  //we need to store a few extra flags in some unused bits in the first pointer of each quadpack
  for(int i=0;4*i<num;i++) {
    list->tris[i].setTriFlag();
  }
  list->tris[(num-1)/4].setEndFlag();
#endif //USE_COMPILED_SHARED_TRIANGLE
  usedTriSlots += num;
  unusedTriSlots += (4 - num)&0x03;
  return (PrimitiveList*)list;
}

void Triangle::expandBoxToInclude(AxisAlignedBox *box) const 
{
  box->addPoint(getV0());
  box->addPoint(getV1());
  box->addPoint(getV2());
}

AxisAlignedBox Triangle::getBoundingBox() const 
{
  F32vec4 v0 = getV0();
  F32vec4 v1 = getV1();
  F32vec4 v2 = getV2();
  return AxisAlignedBox(simd_min(v0,simd_min(v1,v2)),simd_max(v0,simd_max(v1,v2)));
}

bool Triangle::intersectsRay(const Ray &ray, IntersectionRecord *hitRec) const
{
  //first form edges
  F32vec4 e1 = getV1() - getV0();
  F32vec4 e2 = getV2() - getV0();
  //C is e1 X e2
  F32vec4 C = crossProduct(e1,e2);
  F32vec1 denom = rcp_nr(dotProduct(C,ray.direction));
//  F32vec1 denom = F32vec1(1.0f)/dotProduct(C,ray.direction);
  //S is vertex zero minus the ray origin
  F32vec4 S = getV0() - ray.origin;
  //t is the parametric parameter along the ray of the intersection
  //t = (S dot C) / (D dot C)
  F32vec1 t = dotProduct(S,C)*denom;
  //B is S X D
  F32vec4 B = crossProduct(S,ray.direction);
  //u is the first barycentric coordinate of the intersection
  //u = (e2 dot B) / (D dot C)
  F32vec1 u = dotProduct(e2,B)*denom;
  //v is the second barycentric coorinate of the intersection
  //v = -(e1 dot B) / (D dot C)
  F32vec1 negV = dotProduct(e1,B)*denom;
  //test to see if intersection is valid
  F32vec1 zero(0.0f);
  F32vec1 one(1.0f);
  F32vec1 test = cmpge(u,zero)&cmple(negV,zero)&cmple(u-negV,one)
    &cmpge(t,F32vec1(ray.tStart))&cmple(t,F32vec1(ray.tEnd));
  //I haven't thought of a better way of extracting mask from F32vec1 so we convert it to
  //a F32vec4, move the mask to an integer and then ignore everything except the low bit
  if (move_mask(F32vec4(test))&0x01) {
    Store_F32vec1(&hitRec->t,t);
    hitRec->primitiveId = id();
    Store_F32vec1(&hitRec->u,u);
    Store_F32vec1(&hitRec->v,zero-negV);
    hitRec->w = 0;
    return true;
  }
  return false;
}

bool Triangle::intersectsBox(const AxisAlignedBox &box) const
{
  //trivial accept if box contains any of the vertices
  if (box.containsPoint(getV2())
    ||box.containsPoint(getV1())
    ||box.containsPoint(getV0())) return true;
  //trivial reject if box does not intersect bounding box of triangle
  if(!getBoundingBox().intersects(box)) return false;
  //if simplistic tests were not definitive, we use the more expensive exact test
  Ray ray;
  IntersectionRecord hitRec;
  //get triangle edge vectors and take their cross product
  F32vec4 e1 = getV1() - getV0();
  F32vec4 e2 = getV2() - getV0();
  F32vec4 C = crossProduct(e1,e2);
  //now form the diagnonal of the bounding box that is closest to the plane normal
  F32vec4 posMask = cmpge(C,F32vec4Zero());
  F32vec4 S = Select(box.min,box.max,posMask);
  F32vec4 E = Select(box.max,box.min,posMask);
  //and test to see if this diagonal intersects the triangle
  ray.setAsSegment(S,E);
  if (intersectsRay(ray,&hitRec)) return true;
  //next check to see if any of the triangles edges intersect the box
  ray.setAsSegment(getV2(),getV1());
  if (box.clipRay(&ray)) return true;
  ray.setAsSegment(getV2(),getV0());
  if (box.clipRay(&ray)) return true;
  ray.setAsSegment(getV1(),getV0());
  if (box.clipRay(&ray)) return true;
  //otherwise the box does not intersect this triangle
  return false;
}

//used to cache information about a triangle for fast repeated intersection in the next routine
class TriangleIntersectCache {
public:
  F32vec4 v0; //vertex zero
  F32vec4 e1; //edge 1
  F32vec4 e2; //edge 2
  F32vec4 C;  //e1 X e2
  F32vec4 invDC; //1.0 / (C*(max-min))   components used as denominator for axis-aligned edge vs. triangle intersection
};
//fast ray triangle intersection routine for when some quantities can be precomputed and reused for 
//multiple queries against the same triangle.  We check a box edge encoded as a ray (actually line)
//for intersection with the triange.  It it does intersect, then we add the intersection point to
//the AxisAlignedBounding box.
static void boxEdgeIntersectsTriangleFast(const Ray &ray, const TriangleIntersectCache &cached, int axis, AxisAlignedBox &box)
{
//  F32vec1 denom = rcp_nr(dotProduct(cached.C,ray.direction));
  F32vec1 denom = F32vec1(cached.invDC[axis]);
//  F32vec1 denom = F32vec1(1.0f)/dotProduct(C,ray.direction);
  //S is vertex zero minus the ray origin
  F32vec4 S = cached.v0 - ray.origin;
  //B is S X D
  F32vec4 B = crossProduct(S,ray.direction);
  //u is the first barycentric coordinate of the intersection
  //u = (e2 dot B) / (D dot C)
  F32vec1 u = dotProduct(cached.e2,B)*denom;
  //v is the second barycentric coorinate of the intersection
  //v = -(e1 dot B) / (D dot C)
  F32vec1 negV = dotProduct(cached.e1,B)*denom;
  //test to see if intersection is valid
  F32vec1 zero = F32vec1Zero();
  F32vec1 one(1.0f);
//  F32vec1 test = cmpge(u,zero)&cmple(negV,zero)&cmple(u-negV,one);
//  if (move_mask(F32vec4(test))&0x01) {
//  if (_mm_ucomilt_ss(test,zero)) {
  //I apologize for this convoluted expression.  We want to know if the result
  //of the compares is masks of ones (encoded as a -QNAN).  We could do this
  //quickly with an unordered compare, but Intel has not provided an intrinsic for
  //that instruction, so instead I have inverted the tests and used a greater-than-equal 
  //compare which will suceed when both of its arguments are zero and 
  //fail if either argument is a QNAN (such as a mask of all ones)
  if (_mm_ucomige_ss(cmplt(u,zero)|cmpgt(negV,zero),cmpgt(u-negV,one))) {
    //t is the parametric parameter along the ray of the intersection
    //t = (S dot C) / (D dot C)
    F32vec1 t = dotProduct(S,cached.C)*denom;
    F32vec4 pt = ray.origin + ray.direction*BroadcastFloat0(F32vec4(t));
    box.addPoint(pt);
  }
}

//test an edge of an AxisAlignedBox for intersection with triangle and if found,
//add intersection point to bounding box "bounds"
static void
testBoxEdge(AxisAlignedBox &bounds, const F32vec4 &start, const F32vec4 &end,
            const TriangleIntersectCache &cached, int axis)
{
  assert( ((start[0]==end[0])&&(0!=axis)) || ((start[0]<end[0])&&(0==axis)) );
  assert( ((start[1]==end[1])&&(1!=axis)) || ((start[1]<end[1])&&(1==axis)) );
  assert( ((start[2]==end[2])&&(2!=axis)) || ((start[2]<end[2])&&(2==axis)) );
  Ray ray;
  ray.setAsSegment(start,end);
  //we have already proved that the end points lie on opposite side of the triangle's plane
  //therefore the t value should always be between 0 and 1.  But for some reason, probably
  //roundoff error, it ends up slightly outside this range and is false rejected as not
  //hitting the triangles plane.  We avoid this by forcing an extended t range for the
  //test ray
  //ray.tStart = -0.01f;
  //ray.tEnd = 1.01f; 
  boxEdgeIntersectsTriangleFast(ray,cached,axis,bounds);
}
//test all 12 edges of axis aligned box ("box") against the specified triangle 
//and adds any intersection points found to the bounding box ("bounds")
//This routine looks a little more complicated than it is because we have rewritten it
//to make extensive use of SSE simd instructions, but the idea is still the same
static void testBoxEdgesAgainstTriangle(const Triangle *tri, const AxisAlignedBox &box, AxisAlignedBox &bounds)
{
  //precompute some values that we will use in intersection testing against the triangle
  TriangleIntersectCache cached;
//  {
    const Is32vec4 mask3Int(0,-1,-1,-1);
    const F32vec4 mask3 = _mm_castsi128_ps(mask3Int);
    F32vec4 v0 = tri->getV0();
    F32vec4 e1 = tri->getV1() - v0;
    F32vec4 e2 = tri->getV2() - v0;
    F32vec4 C = crossProduct(e1,e2);
    cached.v0 = v0&mask3;
    cached.e1 = e1&mask3;
    cached.e2 = e2&mask3;
    cached.C = C;
    //edges of box will be potential ray directions for intersection query
    //in each case only one component will be non-zero (they're axis aligned)
    //so we can precompute all values of denom = 1.0/dot(C,ray->direction)
    //in the components of 1.0/(C*(max-min)) and we don't have to compute
    //denom for each box-edge vs. triangle intersection query
    cached.invDC = rcp_nr(C*(box.max-box.min));
//  }
  const F32vec4 xMask = _mm_castsi128_ps(brite::VEC_0_0_0_N1);
  const F32vec4 yMask = _mm_castsi128_ps(brite::VEC_0_0_N1_0);
  const F32vec4 zMask = _mm_castsi128_ps(brite::VEC_0_N1_0_0);
  //use crossproduct to get plane equation and then test each of the boxes eight vertices
  //to see which side of the triangles plane they lie on.  Then we only need to test
  //box edges whose endpoints lie on opposite sides of the plane
  F32vec4 minC = box.min*cached.C;
  F32vec4 maxC = box.max*cached.C;
  F32vec4 D = BroadcastFloat0(F32vec4(dotProduct(cached.C,cached.v0)));
  F32vec4 minmaxXminmaxY = unpack_low(minC,maxC);
  F32vec4 minmaxminmaxX = _mm_shuffle_ps(minmaxXminmaxY,minmaxXminmaxY,_MM_SHUFFLE(1,0,1,0));
  F32vec4 minminmaxmaxY = _mm_shuffle_ps(minmaxXminmaxY,minmaxXminmaxY,_MM_SHUFFLE(3,3,2,2));
  F32vec4 side0123 = BroadcastFloat2(minC) - D + minmaxminmaxX + minminmaxmaxY;
  F32vec4 side4567 = BroadcastFloat2(maxC) - D + minmaxminmaxX + minminmaxmaxY;
//  float side[8];
//  float D;
//  Store_F32vec1(&D,dotProduct(cached.C,cached.v0));
//  side[0] = minC[0] + minC[1] + minC[2] - D;
//  side[1] = maxC[0] + minC[1] + minC[2] - D;
//  side[2] = minC[0] + maxC[1] + minC[2] - D;
//  side[3] = maxC[0] + maxC[1] + minC[2] - D;
//  side[4] = minC[0] + minC[1] + maxC[2] - D;
//  side[5] = maxC[0] + minC[1] + maxC[2] - D;
//  side[6] = minC[0] + maxC[1] + maxC[2] - D;
//  side[7] = maxC[0] + maxC[1] + maxC[2] - D;
  //for each box edge that crosses the plane, test it for intersection with the triangle
  F32vec4 side0246 = _mm_shuffle_ps(side0123,side4567,_MM_SHUFFLE(2,0,2,0));
  F32vec4 side1357 = _mm_shuffle_ps(side0123,side4567,_MM_SHUFFLE(3,1,3,1));
  F32vec4 zero = F32vec4Zero();
  int testX = move_mask(cmple(side0246*side1357,zero)&cmpneq(side0246,side1357));
  F32vec4 side0145 = _mm_shuffle_ps(side0123,side4567,_MM_SHUFFLE(1,0,1,0));
  F32vec4 side2367 = _mm_shuffle_ps(side0123,side4567,_MM_SHUFFLE(3,2,3,2));
  int testY = move_mask(cmple(side0145*side2367,zero)&cmpneq(side0145,side2367));
  int testZ = move_mask(cmple(side0123*side4567,zero)&cmpneq(side0123,side4567));

  if (testX&0x01) {
//  if ((side[0]*side[1]<=0)&&(side[0]!=side[1])) {
//    testBoxEdge(bounds,box.min[0],box.min[1],box.min[2],
//                           box.max[0],box.min[1],box.min[2],cached,0);
    testBoxEdge(bounds,box.min,
                          Select(box.max,box.min,xMask),cached,0);
  }
  if (testX&0x02) {
//  if ((side[2]*side[3]<=0)&&(side[2]!=side[3])) {
//    testBoxEdge(bounds,box.min[0],box.max[1],box.min[2],
//                           box.max[0],box.max[1],box.min[2],cached,0);
    testBoxEdge(bounds,Select(box.max,box.min,yMask),
                           Select(box.min,box.max,zMask),cached,0);
  }
  if (testX&0x04) {
//  if ((side[4]*side[5]<=0)&&(side[4]!=side[5])) {
//    testBoxEdge(bounds,box.min[0],box.min[1],box.max[2],
//                           box.max[0],box.min[1],box.max[2],cached,0);
    testBoxEdge(bounds,Select(box.max,box.min,zMask),
                           Select(box.min,box.max,yMask),cached,0);
  }
  if (testX&0x08) {
//  if ((side[6]*side[7]<=0)&&(side[6]!=side[7])) {
//    testBoxEdge(bounds,box.min[0],box.max[1],box.max[2],
//                           box.max[0],box.max[1],box.max[2],cached,0);
    testBoxEdge(bounds,Select(box.min,box.max,xMask),
                           box.max,cached,0);
  }

  if (testY&0x01) {
//  if ((side[0]*side[2]<=0)&&(side[0]!=side[2])) {
//    testBoxEdge(bounds,box.min[0],box.min[1],box.min[2],
//                           box.min[0],box.max[1],box.min[2],cached,1);
    testBoxEdge(bounds,box.min,
                           Select(box.max,box.min,yMask),cached,1);
  }
  if (testY&0x02) {
//  if ((side[1]*side[3]<=0)&&(side[1]!=side[3])) {
//    testBoxEdge(bounds,box.max[0],box.min[1],box.min[2],
//                           box.max[0],box.max[1],box.min[2],cached,1);
    testBoxEdge(bounds,Select(box.max,box.min,xMask),
                           Select(box.min,box.max,zMask),cached,1);
  }
  if (testY&0x04) {
//  if ((side[4]*side[6]<=0)&&(side[4]!=side[6])) {
//    testBoxEdge(bounds,box.min[0],box.min[1],box.max[2],
//                           box.min[0],box.max[1],box.max[2],cached,1);
    testBoxEdge(bounds,Select(box.max,box.min,zMask),
                           Select(box.min,box.max,xMask),cached,1);
  }
  if (testY &0x08) {
//  if ((side[5]*side[7]<=0)&&(side[5]!=side[7])) {
//    testBoxEdge(bounds,box.max[0],box.min[1],box.max[2],
//                           box.max[0],box.max[1],box.max[2],cached,1);
    testBoxEdge(bounds,Select(box.min,box.max,yMask),
                           box.max,cached,1);
  }

  if (testZ&0x01) {
//  if ((side[0]*side[4]<=0)&&(side[0]!=side[4])) {
//    testBoxEdge(bounds,box.min[0],box.min[1],box.min[2],
//                           box.min[0],box.min[1],box.max[2],cached,2);
    testBoxEdge(bounds,box.min,
                           Select(box.max,box.min,zMask),cached,2);
  }
  if (testZ&0x02) {
//  if ((side[1]*side[5]<=0)&&(side[1]!=side[5])) {
//    testBoxEdge(bounds,box.max[0],box.min[1],box.min[2],
//                           box.max[0],box.min[1],box.max[2],cached,2);
    testBoxEdge(bounds,Select(box.max,box.min,xMask),
                           Select(box.min,box.max,yMask),cached,2);
  }
  if (testZ&0x04) {
//  if ((side[2]*side[6]<=0)&&(side[2]!=side[6])) {
//    testBoxEdge(bounds,box.min[0],box.max[1],box.min[2],
//                           box.min[0],box.max[1],box.max[2],cached,2);
    testBoxEdge(bounds,Select(box.max,box.min,yMask),
                           Select(box.min,box.max,xMask),cached,2);
  }
  if (testZ&0x08) {
//  if ((side[3]*side[7]<=0)&&(side[3]!=side[7])) {
//    testBoxEdge(bounds,box.max[0],box.max[1],box.min[2],
//                           box.max[0],box.max[1],box.max[2],cached,2);
    testBoxEdge(bounds,Select(box.min,box.max,zMask),
                           box.max,cached,2);
  }
}

AxisAlignedBox Triangle::estimateIntersectionBounds(const AxisAlignedBox &box) const
{
  AxisAlignedBox bounds = getBoundingBox();
  if (!box.contains(bounds)) {
    //if triangle is not contained in the specified bounding box, then we use a
    //more expensive method to compute the bounding box of the portion of the 
    //triangle that does lie within the specified bounding box
    Ray ray;
    bounds.resetToEmpty(); 
    //form bounding box of portion of triangle inside splits bounding box
    //by finding all possible extremal points and adding them to "bounds"
    //first we test for intersections of the triangles edges with the box
    ray.setAsSegment(getV2(),getV1());
    if (box.clipRay(&ray)) {
      bounds.addPoint(ray.getStartPoint());
      bounds.addPoint(ray.getEndPoint());
    }
    ray.setAsSegment(getV2(),getV0());
    if (box.clipRay(&ray)) {
      bounds.addPoint(ray.getStartPoint());
      bounds.addPoint(ray.getEndPoint());
    }
    ray.setAsSegment(getV1(),getV0());
    if (box.clipRay(&ray)) {
      bounds.addPoint(ray.getStartPoint());
      bounds.addPoint(ray.getEndPoint());
    }
    //next test edges of the box for intersection with the triangle
    testBoxEdgesAgainstTriangle(this,box,bounds);
    if (bounds.isEmpty()) {
      //should never happen as triangle is supposed to be inside these bounds
      //just punt and use the full bounding box for the triangle
      printf("Triangle.estimateIntersectionBounds failed, will use full bounding box\n");
      bounds = getBoundingBox();
//__asm int 3;  //this should never happen, otherwise we get an infinite recursion
//      estimateIntersectionBounds(box);
    }
    assert(!bounds.isEmpty());
    //now we have the exact bounding box for portion of triangle inside splits so add it
  }
  return bounds;
}


void QuadPackedTriangleList::setTriangle(int index, Triangle *from, PoolAllocator *pool)
{
  int i = index/4;
  int j = index & 0x03;

#ifdef USE_COMPILED_SHARED_TRIANGLE

  CompiledSharedTriangle *comp = from->compiledTri;
  if (comp==NULL) {
    comp = (CompiledSharedTriangle*)pool->allocate(sizeof(*comp),SIMD_SIZE);
    comp->set(from);
    from->compiledTri = comp;
  }
  tris[i].tri[j] = comp;

#else //USE_COMPILED_SHARED_TRIANGLE

  assert((length > index)&&(index>=0));
  F32vec4 v0 = from->getV0();
  F32vec4 e1 = from->getV1() - v0;
  F32vec4 e2 = from->getV2() - v0;
  tris[i].v0x[j] = v0[0];
  tris[i].v0y[j] = v0[1];
  tris[i].v0z[j] = v0[2];
  tris[i].e1x[j] = e1[0];
  tris[i].e1y[j] = e1[1];
  tris[i].e1z[j] = e1[2];
  tris[i].e2x[j] = e2[0];
  tris[i].e2y[j] = e2[1];
  tris[i].e2z[j] = e2[2];
  tris[i].id[j] = from->id();

#endif //USE_COMPILED_SHARED_TRIANGLE

}


